/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;
import Behaviors.Schemas.Perception.NxPSMapVP;
import Utils.VectorFrame;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Robot.*;
import Robot.Sensors.*;
import javaclient3.structures.PlayerConstants;
import Utils.*;
import javaclient3.*;
import javax.swing.*;

/**
 *
 * @author alejo
 */
public class mobileSensor {

    private static final int numPoints = 16;

    public static void main(String args[])
    {

        boolean showArbFrame = true;
        boolean showMapObstFrame = true;



        Vector2d[] pointsArray = new Vector2d[200];

        pointsArray[0] = new Vector2d(0.0f,0.0f,Vector2d.MODE_XY);
        pointsArray[1] = new Vector2d(2.4f,0.0f,Vector2d.MODE_XY);
        pointsArray[2] = new Vector2d(2.4f,0.6f,Vector2d.MODE_XY);
        pointsArray[3] = new Vector2d(0.0f,0.6f,Vector2d.MODE_XY);
        pointsArray[4] = new Vector2d(0.0f,1.2f,Vector2d.MODE_XY);
        pointsArray[5] = new Vector2d(2.4f,1.2f,Vector2d.MODE_XY);
        pointsArray[6] = new Vector2d(2.4f,1.8f,Vector2d.MODE_XY);
        pointsArray[7] = new Vector2d(0.0f,1.8f,Vector2d.MODE_XY);
        pointsArray[8] = new Vector2d(0.0f,2.4f,Vector2d.MODE_XY);
        pointsArray[9] = new Vector2d(2.4f,2.4f,Vector2d.MODE_XY);
        pointsArray[10] = new Vector2d(2.4f,1.8f,Vector2d.MODE_XY);
        pointsArray[11] = new Vector2d(0.0f,1.8f,Vector2d.MODE_XY);
        pointsArray[12] = new Vector2d(0.0f,1.2f,Vector2d.MODE_XY);
        pointsArray[13] = new Vector2d(2.4f,1.2f,Vector2d.MODE_XY);
        pointsArray[14] = new Vector2d(2.4f,0.6f,Vector2d.MODE_XY);
        pointsArray[15] = new Vector2d(0.0f,0.6f,Vector2d.MODE_XY);




        Robot real;
        Robot camera;

        //Creacion de la instancia robot.
        real = new Robot("real","192.168.1.102",6665, 0);
        camera = new Robot("camera","localhost",6665, 1);
        
        RealPosition2d posReal;
        RealPosition2d posCamera;
        
        
        try{
        
            posReal = new RealPosition2d(real,"realPos",0);
            posCamera = new RealPosition2d(camera,"cameraPos",0);

    
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }

        Sensor ir;

        try
        {
            ir = new IR(real,"realIR",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }

           

        real.requestDataDeliveryMode(PlayerConstants.PLAYER_DATAMODE_PULL);
        camera.requestDataDeliveryMode(PlayerConstants.PLAYER_DATAMODE_PULL);

        real.runThreaded(-1, -1);
        camera.runThreaded(-1, -1);
        while(!((IR)ir).updatePoses());

        
        posReal.setMotorPower(true);

        Pose2d initialPose;
        initialPose = new Pose2d(0.0f,0.0f,0);

        posReal.setPosition(initialPose);

        posReal.setMaxSpeed(1.0f);
        posReal.setRotControl(0.2f,0.5f);
        


        Vector2d point;

        point=new Vector2d(0.0f,0.0f,Vector2d.MODE_XY);

        NxMSGoToPoint goto2 = new NxMSGoToPoint("goto",posCamera,point,NxMSGoToPoint.MODE_EXPONENTIAL);


        NxPSDetectObstacles obstReal;

        obstReal = new NxPSDetectObstaclesIR("obstReal",posReal, (IR)ir);

        NxPSMapObstacles obstMapped = new NxPSMapObstacles("6mapObstacles",obstReal, posReal,showMapObstFrame);
        
        NxPSMapObstacles obstMapped2 = new NxPSMapObstacles("mapObstacles",obstReal, posReal,0.3f,-1,showMapObstFrame);
        
        
        NxMSAvoidObstacles avoidReal = new NxMSAvoidObstacles("avoidReal",obstMapped2, posReal,NxMSAvoidObstacles.MODE_EXPONENTIAL);


        avoidReal.setExponentialDistance(0.3f);

        NxMSNavigationVFH avoid = new NxMSNavigationVFH("avoidVFH",obstMapped,goto2,posReal,NxMSAvoidObstacles.MODE_LINEAR,true);

        NxMSNoise noise = new NxMSNoise("noise");
        
        goto2.setGain(0.7f);
        avoidReal.setGain(0.8f);
        avoid.setGain(0.7f);
        noise.setGain(0.4f);


        Behavior[] array = {goto2,avoidReal};
        //Behavior[] array = {forward,vanishPoint};//,avoidReal};

        CooperativeArbitrator arbitrator1=new CooperativeArbitrator("a1",array,posReal,posCamera,showArbFrame);
        
        arbitrator1.setDebugMode(true);
        arbitrator1.enabled = true;

        try
        {
        Thread.sleep(2000);
        }
        catch(Exception e)
        {
            e.printStackTrace();
        }

        

        int i = 0;
        while(true)
        {
            arbitrator1.step();
            double distToGoal = goto2.Value().getMag();


            if(distToGoal <0.1f)
            {
                goto2.changeGoalPoint(pointsArray[i++]);

                if(i >= numPoints)
                    i = 0;
            }

            try
            {
                Thread.sleep(50);
            }
            catch(Exception e)
            {}
        }
        
    }

}
